/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the
use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software in a
product, an acknowledgment in the product documentation would be appreciated
but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

/*
GJK-EPA collision solver by Nathanael Presson, 2008
*/
#include "btConvexInternalShape.h"
#include "btSphereShape.h"
#include "btGjkEpa2.h"

#if defined(DEBUG) || defined (_DEBUG)
#include <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#endif //__SPU__
#endif

namespace gjkepa2_impl
{

    // Config

    /* GJK    */
#define GJK_MAX_ITERATIONS    128
#define GJK_ACCURARY        ((btScalar)0.0001)
#define GJK_MIN_DISTANCE    ((btScalar)0.0001)
#define GJK_DUPLICATED_EPS    ((btScalar)0.0001)
#define GJK_SIMPLEX2_EPS    ((btScalar)0.0)
#define GJK_SIMPLEX3_EPS    ((btScalar)0.0)
#define GJK_SIMPLEX4_EPS    ((btScalar)0.0)

    /* EPA    */
#define EPA_MAX_VERTICES    64
#define EPA_MAX_FACES        (EPA_MAX_VERTICES*2)
#define EPA_MAX_ITERATIONS    255
#define EPA_ACCURACY        ((btScalar)0.0001)
#define EPA_FALLBACK        (10*EPA_ACCURACY)
#define EPA_PLANE_EPS        ((btScalar)0.00001)
#define EPA_INSIDE_EPS        ((btScalar)0.01)


    // Shorthands
    typedef unsigned int    U;
    typedef unsigned char    U1;

    // MinkowskiDiff
    struct    MinkowskiDiff
    {
        const btConvexShape*    m_shapes[2];
        btMatrix3x3                m_toshape1;
        btTransform                m_toshape0;
        btVector3                (btConvexShape::*Ls)(const btVector3&) const;
        void                    EnableMargin(bool enable)
        {
            if(enable)
                Ls=&btConvexShape::localGetSupportVertexNonVirtual;
            else
                Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
        }
        inline btVector3        Support0(const btVector3& d) const
        {
            return(((m_shapes[0])->*(Ls))(d));
        }
        inline btVector3        Support1(const btVector3& d) const
        {
            return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
        }
        inline btVector3        Support(const btVector3& d) const
        {
            return(Support0(d)-Support1(-d));
        }
        btVector3                Support(const btVector3& d,U index) const
        {
            if(index)
                return(Support1(d));
            else
                return(Support0(d));
        }
    };

    typedef    MinkowskiDiff    tShape;


    // GJK
    struct    GJK
    {
        /* Types        */
        struct    sSV
        {
            btVector3    d,w;
        };
        struct    sSimplex
        {
            sSV*        c[4];
            btScalar    p[4];
            U            rank;
        };
        struct    eStatus    { enum _ {
            Valid,
            Inside,
            Failed        };};
            /* Fields        */
            tShape            m_shape;
            btVector3        m_ray;
            btScalar        m_distance;
            sSimplex        m_simplices[2];
            sSV                m_store[4];
            sSV*            m_free[4];
            U                m_nfree;
            U                m_current;
            sSimplex*        m_simplex;
            eStatus::_        m_status;
            /* Methods        */
            GJK()
            {
                Initialize();
            }
            void                Initialize()
            {
                m_ray        =    btVector3(0,0,0);
                m_nfree        =    0;
                m_status    =    eStatus::Failed;
                m_current    =    0;
                m_distance    =    0;
            }
            eStatus::_            Evaluate(const tShape& shapearg,const btVector3& guess)
            {
                U            iterations=0;
                btScalar    sqdist=0;
                btScalar    alpha=0;
                btVector3    lastw[4];
                U            clastw=0;
                /* Initialize solver        */
                m_free[0]            =    &m_store[0];
                m_free[1]            =    &m_store[1];
                m_free[2]            =    &m_store[2];
                m_free[3]            =    &m_store[3];
                m_nfree                =    4;
                m_current            =    0;
                m_status            =    eStatus::Valid;
                m_shape                =    shapearg;
                m_distance            =    0;
                /* Initialize simplex        */
                m_simplices[0].rank    =    0;
                m_ray                =    guess;
                const btScalar    sqrl=    m_ray.length2();
                appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
                m_simplices[0].p[0]    =    1;
                m_ray                =    m_simplices[0].c[0]->w;
                sqdist                =    sqrl;
                lastw[0]            =
                    lastw[1]            =
                    lastw[2]            =
                    lastw[3]            =    m_ray;
                /* Loop                        */
                do    {
                    const U        next=1-m_current;
                    sSimplex&    cs=m_simplices[m_current];
                    sSimplex&    ns=m_simplices[next];
                    /* Check zero                            */
                    const btScalar    rl=m_ray.length();
                    if(rl<GJK_MIN_DISTANCE)
                    {/* Touching or inside                */
                        m_status=eStatus::Inside;
                        break;
                    }
                    /* Append new vertice in -'v' direction    */
                    appendvertice(cs,-m_ray);
                    const btVector3&    w=cs.c[cs.rank-1]->w;
                    bool                found=false;
                    for(U i=0;i<4;++i)
                    {
                        if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
                        { found=true;break; }
                    }
                    if(found)
                    {/* Return old simplex                */
                        removevertice(m_simplices[m_current]);
                        break;
                    }
                    else
                    {/* Update lastw                    */
                        lastw[clastw=(clastw+1)&3]=w;
                    }
                    /* Check for termination                */
                    const btScalar    omega=dot(m_ray,w)/rl;
                    alpha=btMax(omega,alpha);
                    if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
                    {/* Return old simplex                */
                        removevertice(m_simplices[m_current]);
                        break;
                    }
                    /* Reduce simplex                        */
                    btScalar    weights[4];
                    U            mask=0;
                    switch(cs.rank)
                    {
                    case    2:    sqdist=projectorigin(    cs.c[0]->w,
                                    cs.c[1]->w,
                                    weights,mask);break;
                    case    3:    sqdist=projectorigin(    cs.c[0]->w,
                                    cs.c[1]->w,
                                    cs.c[2]->w,
                                    weights,mask);break;
                    case    4:    sqdist=projectorigin(    cs.c[0]->w,
                                    cs.c[1]->w,
                                    cs.c[2]->w,
                                    cs.c[3]->w,
                                    weights,mask);break;
                    }
                    if(sqdist>=0)
                    {/* Valid    */
                        ns.rank        =    0;
                        m_ray        =    btVector3(0,0,0);
                        m_current    =    next;
                        for(U i=0,ni=cs.rank;i<ni;++i)
                        {
                            if(mask&(1<<i))
                            {
                                ns.c[ns.rank]        =    cs.c[i];
                                ns.p[ns.rank++]        =    weights[i];
                                m_ray                +=    cs.c[i]->w*weights[i];
                            }
                            else
                            {
                                m_free[m_nfree++]    =    cs.c[i];
                            }
                        }
                        if(mask==15) m_status=eStatus::Inside;
                    }
                    else
                    {/* Return old simplex                */
                        removevertice(m_simplices[m_current]);
                        break;
                    }
                    m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
                } while(m_status==eStatus::Valid);
                m_simplex=&m_simplices[m_current];
                switch(m_status)
                {
                case    eStatus::Valid:        m_distance=m_ray.length();break;
                case    eStatus::Inside:    m_distance=0;break;
                }
                return(m_status);
            }
            bool                    EncloseOrigin()
            {
                switch(m_simplex->rank)
                {
                case    1:
                    {
                        for(U i=0;i<3;++i)
                        {
                            btVector3        axis=btVector3(0,0,0);
                            axis[i]=1;
                            appendvertice(*m_simplex, axis);
                            if(EncloseOrigin())    return(true);
                            removevertice(*m_simplex);
                            appendvertice(*m_simplex,-axis);
                            if(EncloseOrigin())    return(true);
                            removevertice(*m_simplex);
                        }
                    }
                    break;
                case    2:
                    {
                        const btVector3    d=m_simplex->c[1]->w-m_simplex->c[0]->w;
                        for(U i=0;i<3;++i)
                        {
                            btVector3        axis=btVector3(0,0,0);
                            axis[i]=1;
                            const btVector3    p=cross(d,axis);
                            if(p.length2()>0)
                            {
                                appendvertice(*m_simplex, p);
                                if(EncloseOrigin())    return(true);
                                removevertice(*m_simplex);
                                appendvertice(*m_simplex,-p);
                                if(EncloseOrigin())    return(true);
                                removevertice(*m_simplex);
                            }
                        }
                    }
                    break;
                case    3:
                    {
                        const btVector3    n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
                            m_simplex->c[2]->w-m_simplex->c[0]->w);
                        if(n.length2()>0)
                        {
                            appendvertice(*m_simplex,n);
                            if(EncloseOrigin())    return(true);
                            removevertice(*m_simplex);
                            appendvertice(*m_simplex,-n);
                            if(EncloseOrigin())    return(true);
                            removevertice(*m_simplex);
                        }
                    }
                    break;
                case    4:
                    {
                        if(btFabs(det(    m_simplex->c[0]->w-m_simplex->c[3]->w,
                            m_simplex->c[1]->w-m_simplex->c[3]->w,
                            m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
                            return(true);
                    }
                    break;
                }
                return(false);
            }
            /* Internals    */
            void                getsupport(const btVector3& d,sSV& sv) const
            {
                sv.d    =    d/d.length();
                sv.w    =    m_shape.Support(sv.d);
            }
            void                removevertice(sSimplex& simplex)
            {
                m_free[m_nfree++]=simplex.c[--simplex.rank];
            }
            void                appendvertice(sSimplex& simplex,const btVector3& v)
            {
                simplex.p[simplex.rank]=0;
                simplex.c[simplex.rank]=m_free[--m_nfree];
                getsupport(v,*simplex.c[simplex.rank++]);
            }
            static btScalar        det(const btVector3& a,const btVector3& b,const btVector3& c)
            {
                return(    a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
                    a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
                    a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
            }
            static btScalar        projectorigin(    const btVector3& a,
                const btVector3& b,
                btScalar* w,U& m)
            {
                const btVector3    d=b-a;
                const btScalar    l=d.length2();
                if(l>GJK_SIMPLEX2_EPS)
                {
                    const btScalar    t(l>0?-dot(a,d)/l:0);
                    if(t>=1)        { w[0]=0;w[1]=1;m=2;return(b.length2()); }
                    else if(t<=0)    { w[0]=1;w[1]=0;m=1;return(a.length2()); }
                    else            { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
                }
                return(-1);
            }
            static btScalar        projectorigin(    const btVector3& a,
                const btVector3& b,
                const btVector3& c,
                btScalar* w,U& m)
            {
                static const U        imd3[]={1,2,0};
                const btVector3*    vt[]={&a,&b,&c};
                const btVector3        dl[]={a-b,b-c,c-a};
                const btVector3        n=cross(dl[0],dl[1]);
                const btScalar        l=n.length2();
                if(l>GJK_SIMPLEX3_EPS)
                {
                    btScalar    mindist=-1;
                    btScalar    subw[2];
                    U            subm;
                    for(U i=0;i<3;++i)
                    {
                        if(dot(*vt[i],cross(dl[i],n))>0)
                        {
                            const U            j=imd3[i];
                            const btScalar    subd(projectorigin(*vt[i],*vt[j],subw,subm));
                            if((mindist<0)||(subd<mindist))
                            {
                                mindist        =    subd;
                                m            =    static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
                                w[i]        =    subw[0];
                                w[j]        =    subw[1];
                                w[imd3[j]]    =    0;
                            }
                        }
                    }
                    if(mindist<0)
                    {
                        const btScalar    d=dot(a,n);
                        const btScalar    s=btSqrt(l);
                        const btVector3    p=n*(d/l);
                        mindist    =    p.length2();
                        m        =    7;
                        w[0]    =    (cross(dl[1],b-p)).length()/s;
                        w[1]    =    (cross(dl[2],c-p)).length()/s;
                        w[2]    =    1-(w[0]+w[1]);
                    }
                    return(mindist);
                }
                return(-1);
            }
            static btScalar        projectorigin(    const btVector3& a,
                const btVector3& b,
                const btVector3& c,
                const btVector3& d,
                btScalar* w,U& m)
            {
                static const U        imd3[]={1,2,0};
                const btVector3*    vt[]={&a,&b,&c,&d};
                const btVector3        dl[]={a-d,b-d,c-d};
                const btScalar        vl=det(dl[0],dl[1],dl[2]);
                const bool            ng=(vl*dot(a,cross(b-c,a-b)))<=0;
                if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
                {
                    btScalar    mindist=-1;
                    btScalar    subw[3];
                    U            subm;
                    for(U i=0;i<3;++i)
                    {
                        const U            j=imd3[i];
                        const btScalar    s=vl*dot(d,cross(dl[i],dl[j]));
                        if(s>0)
                        {
                            const btScalar    subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
                            if((mindist<0)||(subd<mindist))
                            {
                                mindist        =    subd;
                                m            =    static_cast<U>((subm&1?1<<i:0)+
                                    (subm&2?1<<j:0)+
                                    (subm&4?8:0));
                                w[i]        =    subw[0];
                                w[j]        =    subw[1];
                                w[imd3[j]]    =    0;
                                w[3]        =    subw[2];
                            }
                        }
                    }
                    if(mindist<0)
                    {
                        mindist    =    0;
                        m        =    15;
                        w[0]    =    det(c,b,d)/vl;
                        w[1]    =    det(a,c,d)/vl;
                        w[2]    =    det(b,a,d)/vl;
                        w[3]    =    1-(w[0]+w[1]+w[2]);
                    }
                    return(mindist);
                }
                return(-1);
            }
    };

    // EPA
    struct    EPA
    {
        /* Types        */
        typedef    GJK::sSV    sSV;
        struct    sFace
        {
            btVector3    n;
            btScalar    d;
            btScalar    p;
            sSV*        c[3];
            sFace*        f[3];
            sFace*        l[2];
            U1            e[3];
            U1            pass;
        };
        struct    sList
        {
            sFace*        root;
            U            count;
            sList() : root(0),count(0)    {}
        };
        struct    sHorizon
        {
            sFace*        cf;
            sFace*        ff;
            U            nf;
            sHorizon() : cf(0),ff(0),nf(0)    {}
        };
        struct    eStatus { enum _ {
            Valid,
            Touching,
            Degenerated,
            NonConvex,
            InvalidHull,
            OutOfFaces,
            OutOfVertices,
            AccuraryReached,
            FallBack,
            Failed        };};
            /* Fields        */
            eStatus::_        m_status;
            GJK::sSimplex    m_result;
            btVector3        m_normal;
            btScalar        m_depth;
            sSV                m_sv_store[EPA_MAX_VERTICES];
            sFace            m_fc_store[EPA_MAX_FACES];
            U                m_nextsv;
            sList            m_hull;
            sList            m_stock;
            /* Methods        */
            EPA()
            {
                Initialize();
            }


            static inline void        bind(sFace* fa,U ea,sFace* fb,U eb)
            {
                fa->e[ea]=(U1)eb;fa->f[ea]=fb;
                fb->e[eb]=(U1)ea;fb->f[eb]=fa;
            }
            static inline void        append(sList& list,sFace* face)
            {
                face->l[0]    =    0;
                face->l[1]    =    list.root;
                if(list.root) list.root->l[0]=face;
                list.root    =    face;
                ++list.count;
            }
            static inline void        remove(sList& list,sFace* face)
            {
                if(face->l[1]) face->l[1]->l[0]=face->l[0];
                if(face->l[0]) face->l[0]->l[1]=face->l[1];
                if(face==list.root) list.root=face->l[1];
                --list.count;
            }


            void                Initialize()
            {
                m_status    =    eStatus::Failed;
                m_normal    =    btVector3(0,0,0);
                m_depth        =    0;
                m_nextsv    =    0;
                for(U i=0;i<EPA_MAX_FACES;++i)
                {
                    append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
                }
            }
            eStatus::_            Evaluate(GJK& gjk,const btVector3& guess)
            {
                GJK::sSimplex&    simplex=*gjk.m_simplex;
                if((simplex.rank>1)&&gjk.EncloseOrigin())
                {

                    /* Clean up                */
                    while(m_hull.root)
                    {
                        sFace*    f = m_hull.root;
                        remove(m_hull,f);
                        append(m_stock,f);
                    }
                    m_status    =    eStatus::Valid;
                    m_nextsv    =    0;
                    /* Orient simplex        */
                    if(gjk.det(    simplex.c[0]->w-simplex.c[3]->w,
                        simplex.c[1]->w-simplex.c[3]->w,
                        simplex.c[2]->w-simplex.c[3]->w)<0)
                    {
                        btSwap(simplex.c[0],simplex.c[1]);
                        btSwap(simplex.p[0],simplex.p[1]);
                    }
                    /* Build initial hull    */
                    sFace*    tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
                        newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
                        newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
                        newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
                    if(m_hull.count==4)
                    {
                        sFace*        best=findbest();
                        sFace        outer=*best;
                        U            pass=0;
                        U            iterations=0;
                        bind(tetra[0],0,tetra[1],0);
                        bind(tetra[0],1,tetra[2],0);
                        bind(tetra[0],2,tetra[3],0);
                        bind(tetra[1],1,tetra[3],2);
                        bind(tetra[1],2,tetra[2],1);
                        bind(tetra[2],2,tetra[3],1);
                        m_status=eStatus::Valid;
                        for(;iterations<EPA_MAX_ITERATIONS;++iterations)
                        {
                            if(m_nextsv<EPA_MAX_VERTICES)
                            {
                                sHorizon        horizon;
                                sSV*            w=&m_sv_store[m_nextsv++];
                                bool            valid=true;
                                best->pass    =    (U1)(++pass);
                                gjk.getsupport(best->n,*w);
                                const btScalar    wdist=dot(best->n,w->w)-best->d;
                                if(wdist>EPA_ACCURACY)
                                {
                                    for(U j=0;(j<3)&&valid;++j)
                                    {
                                        valid&=expand(    pass,w,
                                            best->f[j],best->e[j],
                                            horizon);
                                    }
                                    if(valid&&(horizon.nf>=3))
                                    {
                                        bind(horizon.cf,1,horizon.ff,2);
                                        remove(m_hull,best);
                                        append(m_stock,best);
                                        best=findbest();
                                        if(best->p>=outer.p) outer=*best;
                                    } else { m_status=eStatus::InvalidHull;break; }
                                } else { m_status=eStatus::AccuraryReached;break; }
                            } else { m_status=eStatus::OutOfVertices;break; }
                        }
                        const btVector3    projection=outer.n*outer.d;
                        m_normal    =    outer.n;
                        m_depth        =    outer.d;
                        m_result.rank    =    3;
                        m_result.c[0]    =    outer.c[0];
                        m_result.c[1]    =    outer.c[1];
                        m_result.c[2]    =    outer.c[2];
                        m_result.p[0]    =    cross(    outer.c[1]->w-projection,
                            outer.c[2]->w-projection).length();
                        m_result.p[1]    =    cross(    outer.c[2]->w-projection,
                            outer.c[0]->w-projection).length();
                        m_result.p[2]    =    cross(    outer.c[0]->w-projection,
                            outer.c[1]->w-projection).length();
                        const btScalar    sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
                        m_result.p[0]    /=    sum;
                        m_result.p[1]    /=    sum;
                        m_result.p[2]    /=    sum;
                        return(m_status);
                    }
                }
                /* Fallback        */
                m_status    =    eStatus::FallBack;
                m_normal    =    -guess;
                const btScalar    nl=m_normal.length();
                if(nl>0)
                    m_normal    =    m_normal/nl;
                else
                    m_normal    =    btVector3(1,0,0);
                m_depth    =    0;
                m_result.rank=1;
                m_result.c[0]=simplex.c[0];
                m_result.p[0]=1;
                return(m_status);
            }
            sFace*                newface(sSV* a,sSV* b,sSV* c,bool forced)
            {
                if(m_stock.root)
                {
                    sFace*    face=m_stock.root;
                    remove(m_stock,face);
                    append(m_hull,face);
                    face->pass    =    0;
                    face->c[0]    =    a;
                    face->c[1]    =    b;
                    face->c[2]    =    c;
                    face->n        =    cross(b->w-a->w,c->w-a->w);
                    const btScalar    l=face->n.length();
                    const bool        v=l>EPA_ACCURACY;
                    face->p        =    btMin(btMin(
                        dot(a->w,cross(face->n,a->w-b->w)),
                        dot(b->w,cross(face->n,b->w-c->w))),
                        dot(c->w,cross(face->n,c->w-a->w)))    /
                        (v?l:1);
                    face->p        =    face->p>=-EPA_INSIDE_EPS?0:face->p;
                    if(v)
                    {
                        face->d        =    dot(a->w,face->n)/l;
                        face->n        /=    l;
                        if(forced||(face->d>=-EPA_PLANE_EPS))
                        {
                            return(face);
                        } else m_status=eStatus::NonConvex;
                    } else m_status=eStatus::Degenerated;
                    remove(m_hull,face);
                    append(m_stock,face);
                    return(0);
                }
                m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
                return(0);
            }
            sFace*                findbest()
            {
                sFace*        minf=m_hull.root;
                btScalar    mind=minf->d*minf->d;
                btScalar    maxp=minf->p;
                for(sFace* f=minf->l[1];f;f=f->l[1])
                {
                    const btScalar    sqd=f->d*f->d;
                    if((f->p>=maxp)&&(sqd<mind))
                    {
                        minf=f;
                        mind=sqd;
                        maxp=f->p;
                    }
                }
                return(minf);
            }
            bool                expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
            {
                static const U    i1m3[]={1,2,0};
                static const U    i2m3[]={2,0,1};
                if(f->pass!=pass)
                {
                    const U    e1=i1m3[e];
                    if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
                    {
                        sFace*    nf=newface(f->c[e1],f->c[e],w,false);
                        if(nf)
                        {
                            bind(nf,0,f,e);
                            if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
                            horizon.cf=nf;
                            ++horizon.nf;
                            return(true);
                        }
                    }
                    else
                    {
                        const U    e2=i2m3[e];
                        f->pass        =    (U1)pass;
                        if(    expand(pass,w,f->f[e1],f->e[e1],horizon)&&
                            expand(pass,w,f->f[e2],f->e[e2],horizon))
                        {
                            remove(m_hull,f);
                            append(m_stock,f);
                            return(true);
                        }
                    }
                }
                return(false);
            }

    };

    //
    static void    Initialize(    const btConvexShape* shape0,const btTransform& wtrs0,
        const btConvexShape* shape1,const btTransform& wtrs1,
        btGjkEpaSolver2::sResults& results,
        tShape& shape,
        bool withmargins)
    {
        /* Results        */
        results.witnesses[0]    =
            results.witnesses[1]    =    btVector3(0,0,0);
        results.status            =    btGjkEpaSolver2::sResults::Separated;
        /* Shape        */
        shape.m_shapes[0]        =    shape0;
        shape.m_shapes[1]        =    shape1;
        shape.m_toshape1        =    wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
        shape.m_toshape0        =    wtrs0.inverseTimes(wtrs1);
        shape.EnableMargin(withmargins);
    }

}

//
// Api
//

using namespace    gjkepa2_impl;

//
int            btGjkEpaSolver2::StackSizeRequirement()
{
    return(sizeof(GJK)+sizeof(EPA));
}

//
bool        btGjkEpaSolver2::Distance(    const btConvexShape*    shape0,
                                      const btTransform&        wtrs0,
                                      const btConvexShape*    shape1,
                                      const btTransform&        wtrs1,
                                      const btVector3&        guess,
                                      sResults&                results)
{
    tShape            shape;
    Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
    GJK                gjk;
    GJK::eStatus::_    gjk_status=gjk.Evaluate(shape,guess);
    if(gjk_status==GJK::eStatus::Valid)
    {
        btVector3    w0=btVector3(0,0,0);
        btVector3    w1=btVector3(0,0,0);
        for(U i=0;i<gjk.m_simplex->rank;++i)
        {
            const btScalar    p=gjk.m_simplex->p[i];
            w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
            w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
        }
        results.witnesses[0]    =    wtrs0*w0;
        results.witnesses[1]    =    wtrs0*w1;
        results.normal            =    w0-w1;
        results.distance        =    results.normal.length();
        results.normal            /=    results.distance>GJK_MIN_DISTANCE?results.distance:1;
        return(true);
    }
    else
    {
        results.status    =    gjk_status==GJK::eStatus::Inside?
            sResults::Penetrating    :
        sResults::GJK_Failed    ;
        return(false);
    }
}

//
bool    btGjkEpaSolver2::Penetration(    const btConvexShape*    shape0,
                                     const btTransform&        wtrs0,
                                     const btConvexShape*    shape1,
                                     const btTransform&        wtrs1,
                                     const btVector3&        guess,
                                     sResults&                results,
                                     bool                    usemargins)
{
    tShape            shape;
    Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
    GJK                gjk;
    GJK::eStatus::_    gjk_status=gjk.Evaluate(shape,-guess);
    switch(gjk_status)
    {
    case    GJK::eStatus::Inside:
        {
            EPA                epa;
            EPA::eStatus::_    epa_status=epa.Evaluate(gjk,-guess);
            if(epa_status!=EPA::eStatus::Failed)
            {
                btVector3    w0=btVector3(0,0,0);
                for(U i=0;i<epa.m_result.rank;++i)
                {
                    w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
                }
                results.status            =    sResults::Penetrating;
                results.witnesses[0]    =    wtrs0*w0;
                results.witnesses[1]    =    wtrs0*(w0-epa.m_normal*epa.m_depth);
                results.normal            =    -epa.m_normal;
                results.distance        =    -epa.m_depth;
                return(true);
            } else results.status=sResults::EPA_Failed;
        }
        break;
    case    GJK::eStatus::Failed:
        results.status=sResults::GJK_Failed;
        break;
    }
    return(false);
}

//
btScalar    btGjkEpaSolver2::SignedDistance(const btVector3& position,
                                            btScalar margin,
                                            const btConvexShape* shape0,
                                            const btTransform& wtrs0,
                                            sResults& results)
{
    tShape            shape;
    btSphereShape    shape1(margin);
    btTransform        wtrs1(btQuaternion(0,0,0,1),position);
    Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
    GJK                gjk;
    GJK::eStatus::_    gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
    if(gjk_status==GJK::eStatus::Valid)
    {
        btVector3    w0=btVector3(0,0,0);
        btVector3    w1=btVector3(0,0,0);
        for(U i=0;i<gjk.m_simplex->rank;++i)
        {
            const btScalar    p=gjk.m_simplex->p[i];
            w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
            w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
        }
        results.witnesses[0]    =    wtrs0*w0;
        results.witnesses[1]    =    wtrs0*w1;
        const btVector3    delta=    results.witnesses[1]-
            results.witnesses[0];
        const btScalar    margin=    shape0->getMarginNonVirtual()+
            shape1.getMarginNonVirtual();
        const btScalar    length=    delta.length();
        results.normal            =    delta/length;
        results.witnesses[0]    +=    results.normal*margin;
        return(length-margin);
    }
    else
    {
        if(gjk_status==GJK::eStatus::Inside)
        {
            if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
            {
                const btVector3    delta=    results.witnesses[0]-
                    results.witnesses[1];
                const btScalar    length=    delta.length();
                if (length >= SIMD_EPSILON)
                    results.normal    =    delta/length;
                return(-length);
            }
        }
    }
    return(SIMD_INFINITY);
}

//
bool    btGjkEpaSolver2::SignedDistance(const btConvexShape*    shape0,
                                        const btTransform&        wtrs0,
                                        const btConvexShape*    shape1,
                                        const btTransform&        wtrs1,
                                        const btVector3&        guess,
                                        sResults&                results)
{
    if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
        return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
    else
        return(true);
}

/* Symbols cleanup        */

#undef GJK_MAX_ITERATIONS
#undef GJK_ACCURARY
#undef GJK_MIN_DISTANCE
#undef GJK_DUPLICATED_EPS
#undef GJK_SIMPLEX2_EPS
#undef GJK_SIMPLEX3_EPS
#undef GJK_SIMPLEX4_EPS

#undef EPA_MAX_VERTICES
#undef EPA_MAX_FACES
#undef EPA_MAX_ITERATIONS
#undef EPA_ACCURACY
#undef EPA_FALLBACK
#undef EPA_PLANE_EPS
#undef EPA_INSIDE_EPS
